#include "joint_math.h"

#define Fu_isfin(x)		(isfinite(x) != 0 ? x : 0)

/**
 * @brief  机体到地球
 * @param  void
 * @retval void
 */
void point_transform(float *x, float *y, float angle)
{
	float angle_pi = angle / 180.f * 3.141592654f;
	
  /* From body frame to earth frame */
	float tx = *x * cos(angle_pi) + *y * sin(angle_pi);
	float ty = *y * cos(angle_pi) - *x * sin(angle_pi);	
	
	*x = tx;
	*y = ty;
}

/**
 * @brief  运动学逆解
 * @param  
 * @retval 无
 */
void ik(ik_t *ik, union _point_t point)
{
	float x = limit(point.x, 60, -60);
	float y = limit(point.y, 50, -50) - link1_len + 120;
	float z = limit(point.z, 200, -150) - 37.f;
	
	/* 安装误差校正 */ 
	point_transform(&y, &x, ik->joint1_angle_err);
	
	float l0_0 = - link1_len + 120;
	x = x + l0_0 * sin(ik->joint1_angle_err / 180.f * PI);
	y = y + (l0_0 - l0_0 * cos(ik->joint1_angle_err / 180.f * PI));
	
	
	//------ 第0个角度 -------
	// 辅助线
	float l0 = sqrt(pow2(y) - pow2(x));
	float l1 = y - l0;
	
	float theta_0 = Fu_isfin(Cosine_BegAngle(y, l0, x)) * (x / fabs(x));
	
	float offset = sqrt(pow2(l1) + pow2(x));
	float y2 = y + offset;
	
	//------ 第一个角度 -------
	// 辅助线
	float l3 = sqrt(pow2(y2) + pow2(z));
	
	float theta_11 = Cosine_BegAngle(link2_len, l3, link3_len);
	float theta_12 = atan2(z, y2);
	
	float theta_1 = theta_11 + theta_12;
	
	
	//------ 第二个角度 -------
	float theta_2 = PI - Cosine_BegAngle(link2_len, link3_len, l3);
	
	/* 赋值 */
	ik->angle[0] = theta_0;
	ik->angle[1] = theta_1;
	ik->angle[2] = theta_2;
}


/******************* (C) COPYRIGHT 2025 FUYEBING*****END OF FILE************/
